﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

/// <summary>
/// ConsoleApp.DroneCameraImageHelper
/// </summary>

namespace ConsoleApp
{
    public static class GeodeticUtils
    {
        // WGS84椭球参数
        private const double EquatorialRadius = 6378137.0;
        private const double PolarRadius = 6356752.314245;
        private const double EccentricitySquared = 6.69437999014e-3;

        public static (double X, double Y, double Z) GeodeticToEcef(double lat, double lon, double alt)
        {
            double latRad = DegreeToRadian(lat);
            double lonRad = DegreeToRadian(lon);

            double n = EquatorialRadius / Math.Sqrt(1 - EccentricitySquared * Math.Sin(latRad) * Math.Sin(latRad));
            double x = (n + alt) * Math.Cos(latRad) * Math.Cos(lonRad);
            double y = (n + alt) * Math.Cos(latRad) * Math.Sin(lonRad);
            double z = (n * (1 - EccentricitySquared) + alt) * Math.Sin(latRad);

            return (x, y, z);
        }

        public static (double Lat, double Lon, double Alt) EcefToGeodetic(double x, double y, double z)
        {
            double p = Math.Sqrt(x * x + y * y);
            double theta = Math.Atan2(z * EquatorialRadius, p * PolarRadius);

            double lon = Math.Atan2(y, x);
            double lat = Math.Atan2(
                z + (PolarRadius * PolarRadius - EquatorialRadius * EquatorialRadius) /
                    (EquatorialRadius * EquatorialRadius) * PolarRadius * Math.Pow(Math.Sin(theta), 3),
                p - (EquatorialRadius * EquatorialRadius - PolarRadius * PolarRadius) /
                    (PolarRadius * PolarRadius) * EquatorialRadius * Math.Pow(Math.Cos(theta), 3)
            );

            double n = EquatorialRadius / Math.Sqrt(1 - EccentricitySquared * Math.Sin(lat) * Math.Sin(lat));
            double alt = p / Math.Cos(lat) - n;

            return (RadianToDegree(lat), RadianToDegree(lon), alt);
        }

        public static (double X, double Y, double Z) EnuToEcef(
            double east, double north, double up,
            double refLat, double refLon, double refAlt)
        {
            (double refX, double refY, double refZ) = GeodeticToEcef(refLat, refLon, refAlt);

            double latRad = DegreeToRadian(refLat);
            double lonRad = DegreeToRadian(refLon);

            double[][] R = {
                new[] { -Math.Sin(lonRad), -Math.Sin(latRad)*Math.Cos(lonRad), Math.Cos(latRad)*Math.Cos(lonRad) },
                new[] { Math.Cos(lonRad), -Math.Sin(latRad)*Math.Sin(lonRad), Math.Cos(latRad)*Math.Sin(lonRad) },
                new[] { 0, Math.Cos(latRad), Math.Sin(latRad) }
            };

            double dx = R[0][0] * east + R[0][1] * north + R[0][2] * up;
            double dy = R[1][0] * east + R[1][1] * north + R[1][2] * up;
            double dz = R[2][0] * east + R[2][1] * north + R[2][2] * up;

            return (refX + dx, refY + dy, refZ + dz);
        }

        public static double[,] CreateRotationMatrix(double yawDeg, double pitchDeg, double rollDeg)
        {
            double yaw = DegreeToRadian(NormalizeAngle(yawDeg));
            double pitch = DegreeToRadian(NormalizeAngle(pitchDeg));
            double roll = DegreeToRadian(NormalizeAngle(rollDeg));

            double[,] Rz = {
                { Math.Cos(yaw), -Math.Sin(yaw), 0 },
                { Math.Sin(yaw), Math.Cos(yaw), 0 },
                { 0, 0, 1 }
            };

            double[,] Ry = {
                { Math.Cos(pitch), 0, Math.Sin(pitch) },
                { 0, 1, 0 },
                { -Math.Sin(pitch), 0, Math.Cos(pitch) }
            };

            double[,] Rx = {
                { 1, 0, 0 },
                { 0, Math.Cos(roll), -Math.Sin(roll) },
                { 0, Math.Sin(roll), Math.Cos(roll) }
            };

            return MultiplyMatrices(MultiplyMatrices(Rz, Ry), Rx);
        }

        private static double[,] MultiplyMatrices(double[,] a, double[,] b)
        {
            int rows = a.GetLength(0);
            int cols = b.GetLength(1);
            double[,] result = new double[rows, cols];

            for (int i = 0; i < rows; i++)
            {
                for (int j = 0; j < cols; j++)
                {
                    result[i, j] = 0;
                    for (int k = 0; k < a.GetLength(1); k++)
                    {
                        result[i, j] += a[i, k] * b[k, j];
                    }
                }
            }
            return result;
        }

        private static double DegreeToRadian(double deg) => deg * Math.PI / 180.0;
        private static double RadianToDegree(double rad) => rad * 180.0 / Math.PI;

        private static double NormalizeAngle(double angle)
        {
            // 将角度标准化到[-180, 180]范围
            angle %= 360;
            if (angle > 180) angle -= 360;
            if (angle < -180) angle += 360;
            return angle;
        }
    }

    public class DroneCameraImageHelper
    {
        private const double GroundTolerance = 1e-5;

        public static (double Lon, double Lat) CalculateImageCenter(
            double droneLon, double droneLat, double droneAlt,
            double yaw, double pitch, double roll)
        {
            // 1. 获取旋转矩阵
            double[,] R = GeodeticUtils.CreateRotationMatrix(yaw, pitch, roll);

            // 2. 计算相机光轴方向
            double[] bodyDirection = { 0, 0, -1 };

            // 3. 转换到ENU坐标系
            double[] enuDirection = {
                R[0,0]*bodyDirection[0] + R[0,1]*bodyDirection[1] + R[0,2]*bodyDirection[2],
                R[1,0]*bodyDirection[0] + R[1,1]*bodyDirection[1] + R[1,2]*bodyDirection[2],
                R[2,0]*bodyDirection[0] + R[2,1]*bodyDirection[1] + R[2,2]*bodyDirection[2]
            };

            // 4. 处理特殊姿态 - 当滚转180°+俯仰-90°时实际指向水平
            if (Math.Abs(enuDirection[2]) < GroundTolerance)
            {
                // 水平指向时无法确定地面点，返回无人机正下方位置
                return (droneLon, droneLat);
            }

            // 5. 计算与地面交点
            double t = -droneAlt / enuDirection[2];
            double east = enuDirection[0] * t;
            double north = enuDirection[1] * t;

            // 6. 转换到地理坐标
            (double ecefX, double ecefY, double ecefZ) =
                GeodeticUtils.EnuToEcef(east, north, 0, droneLat, droneLon, 0);

            (double lat, double lon, double _) = GeodeticUtils.EcefToGeodetic(ecefX, ecefY, ecefZ);

            return (lon, lat);
        }
    }

  
}
